package test;

import java.util.*;

public class PathPlanner {

    public static class UpPose {
        private double posX;
        private double posY;
        private double yaw;

        UpPose(double posX, double posY, double yaw) {
            this.posX = posX;
            this.posY = posY;
            this.yaw = yaw;
        }

        public double getPosX() {
            return posX;
        }

        public double getPosY() {
            return posY;
        }

        public double getYaw() {
            return yaw;
        }
    }

    public static class PointDto {
        private String id;
        private String name;
        private UpPose upPose;
        private String turnFlg;

        PointDto(String id, String name, UpPose upPose, String turnFlg) {
            this.id = id;
            this.name = name;
            this.upPose = upPose;
            this.turnFlg = turnFlg;
        }

        public String getId() {
            return id;
        }

        public String getName() {
            return name;
        }

        public UpPose getUpPose() {
            return upPose;
        }

        public String getTurnFlg() {
            return turnFlg;
        }
    }

    public static class WayDto {
        private String id;
        private String sourceId;
        private String targetId;
        private int manner;

        private double distance;

        WayDto(String id, String sourceId, String targetId, int manner) {
            this.id = id;
            this.sourceId = sourceId;
            this.targetId = targetId;
            this.manner = manner;
        }

        public String getId() {
            return id;
        }

        public String getSourceId() {
            return sourceId;
        }

        public String getTargetId() {
            return targetId;
        }

        public int getManner() {
            return manner;
        }

        public void setDistance(double distance) {
            this.distance = distance;
        }

        public double getDistance() {
            return distance;
        }

        public String toString(Map<String, PointDto> pointMap) {
            PointDto source = pointMap.get(sourceId);
            PointDto target = pointMap.get(targetId);
            String sourceName = source != null ? source.getName() : "未知";
            String targetName = target != null ? target.getName() : "未知";
            return "[" + sourceName + ", " + targetName + "]";
        }
    }

    public static class PathNode {
        private String id;         // 节点ID
        private String name;
        private double posX;
        private double posY;
        private String turnFlg;
        // 节点ID
        private double distance;       // 累加的距离
        private PathNode prev;    // 前驱节点
        private double currentYaw;
        private boolean turn;

        // 用于起点或优先级队列的节点
        PathNode(PointDto point, double distance, double currentYaw, boolean turn, PathNode prev) {
            this.id = point.getId();
            this.name = point.getName();
            this.posX = point.getUpPose().getPosX();
            this.posY = point.getUpPose().getPosY();
            this.turnFlg = point.getTurnFlg();
            this.distance = distance;
            this.currentYaw = currentYaw;
            this.turn = turn;
            this.prev = prev;
        }

        public String getId() {
            return id;
        }

        public String getName() {
            return name;
        }

        public void setName(String name) {
            this.name = name;
        }

        public double getDistance() {
            return distance;
        }

        public PathNode getPrev() {
            return prev;
        }


        public double getCurrentYaw() {
            return currentYaw;
        }

        public double getPosX() {
            return posX;
        }

        public double getPosY() {
            return posY;
        }

        public String getTurnFlg() {
            return turnFlg;
        }

        public boolean isTurn() {
            return turn;
        }
    }

    private final List<WayDto> ways;
    private final Map<String, PointDto> pointMap = new HashMap<>();
    private final Map<String, List<WayDto>> graph = new HashMap<>();

    public PathPlanner(List<PointDto> points, List<WayDto> ways) {
        for (PointDto p : points) {
            pointMap.put(p.getId(), p);
            graph.putIfAbsent(p.getId(), new ArrayList<>());
        }
        this.ways = ways;
        for (WayDto way : ways) {
            PointDto source = pointMap.get(way.getSourceId());
            PointDto target = pointMap.get(way.getTargetId());
            way.setDistance(calculateDistance(source, target));
            // 构造有向图
            graph.get(way.getSourceId()).add(way);
        }
    }

    private static double calculateDistance(PointDto p1, PointDto p2) {
        if (p1 == null || p2 == null) return 0.0;
        return Math.sqrt(Math.pow(p2.getUpPose().getPosX() - p1.getUpPose().getPosX(), 2) +
                Math.pow(p2.getUpPose().getPosY() - p1.getUpPose().getPosY(), 2));
    }

    public static boolean requiresUTurn(double currentX, double currentY, double currentYaw, double targetX, double targetY) {
        // 计算目标方向（目标yaw）
        double targetYaw = Math.atan2(targetY - currentY, targetX - currentX);

        return !isSamDirection(targetYaw, currentYaw);
    }

    private static boolean isSamDirection(double targetYaw, double currentYaw) {
        // 计算当前yaw和目标yaw之间的角度差异
        double yawDifference = normalizeAngle(targetYaw - currentYaw);

        // 如果角度差异超过 ±2π/3（120度），则需要调头
        return Math.abs(yawDifference) < (2 * Math.PI / 3);
    }

    // 辅助函数：将角度规范化到[-π, π]范围内
    private static double normalizeAngle(double angle) {
        angle = angle % (2 * Math.PI);
        if (angle > Math.PI) {
            angle -= 2 * Math.PI;
        } else if (angle < -Math.PI) {
            angle += 2 * Math.PI;
        }
        return angle;
    }

    public static double turnRoundYaw(double yaw) {
        return yaw > 0 ? yaw - Math.PI : yaw + Math.PI;
    }

    private List<WayDto> convertToWayList(PathNode endNode) {
        List<WayDto> wayList = new ArrayList<>();
        Map<String, WayDto> wayMap = new HashMap<>();
        for (WayDto way : ways) {
            wayMap.put(way.getSourceId() + "->" + way.getTargetId(), way);
        }

        List<String> pointPath = new ArrayList<>();
        PathNode current = endNode;
        while (current != null) {
            pointPath.add(0, current.getId());
            current = current.getPrev();
        }

        for (int i = 0; i < pointPath.size() - 1; i++) {
            String sourceId = pointPath.get(i);
            String targetId = pointPath.get(i + 1);
            String key = sourceId + "->" + targetId;
            WayDto way = wayMap.get(key);
            if (way != null) {
                wayList.add(way);
            } else {
                System.out.println("未找到路径段: " + sourceId + "->" + targetId);
                return null;
            }
        }
        return wayList;
    }

    private List<WayDto> planSingleSegment(PointDto from, PointDto to) {
        Map<String, PathNode> visited = new HashMap<>();
        PriorityQueue<PathNode> pq = new PriorityQueue<>(Comparator.comparingDouble(PathNode::getDistance));
        // 创建起点节点
        PathNode startNode = new PathNode(from, 0.0, from.getUpPose().getYaw(), false, null);
        pq.add(startNode);

        while (!pq.isEmpty()) {
            PathNode currentNode = pq.poll();
            String currentId = currentNode.getId();

            // 如果当前节点为目标节点，并且当前yaw与目标yaw相同，或者turnFlg为1，则返回路径
            if (currentId.equals(to.getId())
                    && (currentNode.getCurrentYaw() == to.getUpPose().getYaw() || currentNode.getTurnFlg().equals("1"))) {
                return convertToWayList(currentNode);
            }

            // 避免重复访问节点
            String key = currentId + "_" + currentNode.getCurrentYaw();
            PathNode visitedNode = visited.get(key);
            if (visitedNode != null && visitedNode.getDistance() < currentNode.getDistance()) {
                continue;
            }
            visited.put(key, currentNode);

            // 遍历当前节点可到达的路径
            for (WayDto way : graph.get(currentId)) {
                PointDto target = pointMap.get(way.getTargetId());

                boolean requiresUTurn = requiresUTurn(currentNode.getPosX(), currentNode.getPosY(),
                        currentNode.getCurrentYaw(), target.getUpPose().getPosX(), target.getUpPose().getPosY());
                // 倒走路径不需要掉头
                requiresUTurn = requiresUTurn && way.getManner() == 0;

                // 需要掉头但当前turnFlg为0，则跳过该路径
                if (requiresUTurn && currentNode.getTurnFlg().equals("0")) {
                    continue;
                }

                // 计算新距离
                double newDistance = currentNode.getDistance() + way.getDistance();

                double originalYaw = target.getUpPose().getYaw();
                double turnRoundYaw = turnRoundYaw(originalYaw);

                PathNode newNode;

                if (requiresUTurn) {
                    if (currentNode.isTurn()) {
                        newNode = new PathNode(target, newDistance, originalYaw, false, currentNode);
                    } else {
                        newNode = new PathNode(target, newDistance, turnRoundYaw, true, currentNode);
                    }
                } else {
                    if (currentNode.isTurn()) {
                        if (!isSamDirection(currentNode.getCurrentYaw(), turnRoundYaw)) {
                            turnRoundYaw = originalYaw;
                        }
                        newNode = new PathNode(target, newDistance, turnRoundYaw, true, currentNode);
                    } else {
                        newNode = new PathNode(target, newDistance, originalYaw, false, currentNode);
                    }
                }
                pq.add(newNode);
            }
        }

        System.out.println("单段路径不可达: " + from.getName() + " -> " + to.getName());
        return new ArrayList<>();
    }

    public List<WayDto> planPath(String startId, List<String> targetIds, String endId) {
        // 初始化关键点
        List<String> keyPoints = new ArrayList<>();
        keyPoints.add(startId);
        keyPoints.addAll(targetIds);
        keyPoints.add(endId);
        int n = keyPoints.size();
        int targetCount = targetIds.size();
        int fullMask = (1 << targetCount) - 1;

        // 预计算所有关键点之间的路径
        Map<String, Map<String, List<WayDto>>> pathMatrix = new HashMap<>();
        for (int i = 0; i < n; i++) {
            String fromId = keyPoints.get(i);
            PointDto fromPoint = pointMap.get(fromId);
            Map<String, List<WayDto>> paths = new HashMap<>();
            for (int j = 0; j < n; j++) {
                if (i == j) continue;
                String toId = keyPoints.get(j);
                PointDto toPoint = pointMap.get(toId);
                List<WayDto> result = planSingleSegment(fromPoint, toPoint);
                if (result != null && !result.isEmpty()) {
                    paths.put(toId, result);
                }
            }
            pathMatrix.put(fromId, paths);
        }

        // 动态规划表
        // dp[i][mask]：到达点 i，已访问目标点状态为 mask 的最小距离，初始为无穷大。
        // prev[i][mask][0]：记录前一个点的索引。
        // prev[i][mask][1]：记录前一个状态的掩码。
        double[][] dp = new double[n][1 << targetCount];
        int[][][] prev = new int[n][1 << targetCount][2];

        // 初始化DP表
        for (int i = 0; i < n; i++) {
            Arrays.fill(dp[i], Double.POSITIVE_INFINITY);
        }
        dp[0][0] = 0.0;

        // 初始化从起点到每个目标点的路径
        for (int j = 1; j <= targetCount; j++) {
            List<WayDto> segment = pathMatrix.getOrDefault(keyPoints.get(0), new HashMap<>()).get(keyPoints.get(j));
            if (segment != null) {
                double segmentDist = segment.stream().mapToDouble(WayDto::getDistance).sum();
                dp[j][1 << (j - 1)] = segmentDist;
                prev[j][1 << (j - 1)][0] = 0;
                prev[j][1 << (j - 1)][1] = 0;
            }
        }

        // 动态规划状态转移
        for (int mask = 0; mask <= fullMask; mask++) {
            for (int i = 0; i < n; i++) {
                if (dp[i][mask] == Double.POSITIVE_INFINITY) continue;
                for (int j = 0; j < n; j++) {
                    if (i == j || (j > 0 && j <= targetCount && (mask & (1 << (j - 1))) != 0)) continue;
                    List<WayDto> segment = pathMatrix.getOrDefault(keyPoints.get(i), new HashMap<>()).get(keyPoints.get(j));
                    if (segment == null) continue;

                    int nextMask = mask;
                    if (j > 0 && j <= targetCount) {
                        nextMask |= (1 << (j - 1));
                    }
                    double segmentDist = segment.stream().mapToDouble(WayDto::getDistance).sum();
                    double newDist = dp[i][mask] + segmentDist;
                    if (newDist < dp[j][nextMask]) {
                        dp[j][nextMask] = newDist;
                        prev[j][nextMask][0] = i;
                        prev[j][nextMask][1] = mask;
                    }
                }
            }
        }

        // 检查是否找到完整路径
        if (dp[n - 1][fullMask] == Double.POSITIVE_INFINITY) {
            return new ArrayList<>();
        }

        // 回溯路径
        List<WayDto> fullWayList = new ArrayList<>();
        int currentIdx = n - 1;
        int currentMask = fullMask;
        while (currentIdx != 0) {
            int prevIdx = prev[currentIdx][currentMask][0];
            int prevMask = prev[currentIdx][currentMask][1];
            String fromId = keyPoints.get(prevIdx);
            String toId = keyPoints.get(currentIdx);
            List<WayDto> segment = pathMatrix.get(fromId).get(toId);
            fullWayList.addAll(0, segment);
            currentIdx = prevIdx;
            currentMask = prevMask;
        }

        return fullWayList;
    }

    public static void main(String[] args) {
        List<PointDto> points = new ArrayList<>();
        points.add(new PointDto("1801430161384710145", "充电点", new UpPose(18.611, -0.303, -0.129), "1"));
        points.add(new PointDto("1801430457993306114", "转弯中途经点-00", new UpPose(20.094, 0.037, 0.779), "0"));
        points.add(new PointDto("1801430840669020162", "转弯后途经点-01", new UpPose(20.536, 0.719, 1.3066554994933677), "0"));
        points.add(new PointDto("1801438932798529538", "局放-巡检作业点", new UpPose(21.224, 2.197, 1.343), "0"));
        points.add(new PointDto("1801441962147065857", "途经点4", new UpPose(21.204, 4.001, 1.3786029981858492), "0"));
        points.add(new PointDto("1801442212664455170", "最后的巡视拍照点", new UpPose(21.436374999999998, 5.29625, 1.35), "0"));
        points.add(new PointDto("1801442421679206402", "掉头点", new UpPose(21.826, 7.352, 1.3632340275098356), "1"));
        points.add(new PointDto("1821071654021984257", "巡检作业点-L02", new UpPose(18.025, 0.06075, -0.102), "0"));
        points.add(new PointDto("1821072243518828546", "巡检作业点--R01", new UpPose(16.076, -0.338, 0.009), "0"));
        points.add(new PointDto("1821073230505668610", "正向起始作业点-00", new UpPose(14.194, -0.265, 0.001), "1"));
        points.add(new PointDto("1821797679173349377", "新加起始点", new UpPose(4.319, -0.112, 3.099), "0"));
        points.add(new PointDto("1821798184951885826", "新加巡视点-01", new UpPose(8.077, -0.142, -0.028), "0"));
        points.add(new PointDto("1821798392075005954", "新加途经点-02", new UpPose(10.517, -0.21, -0.021), "0"));
        points.add(new PointDto("1821803090509180929", "新加巡视点-02", new UpPose(12.38, -0.276, -0.003), "0"));
        points.add(new PointDto("1821817904841240577", "新加途经点-01", new UpPose(5.627, -0.138, -0.022), "1"));
        points.add(new PointDto("1866050332438654978", "充电板", new UpPose(20.274, -0.521, -0.08), "0"));

        Map<String, PointDto> pointMap = new HashMap<>();
        for (PointDto p : points) {
            pointMap.put(p.getId(), p);
        }

        List<WayDto> ways = new ArrayList<>();
        ways.add(new WayDto("1801430893626302466", "1801430457993306114", "1801430840669020162", 0));
        ways.add(new WayDto("1801438956647342082", "1801430840669020162", "1801438932798529538", 0));
        ways.add(new WayDto("1801441980866244609", "1801438932798529538", "1801441962147065857", 0));
        ways.add(new WayDto("1801442237272436737", "1801441962147065857", "1801442212664455170", 0));
        ways.add(new WayDto("1801442435361026049", "1801442212664455170", "1801442421679206402", 0));
        ways.add(new WayDto("1801442479145365505", "1801442421679206402", "1801441962147065857", 0));
        ways.add(new WayDto("1801442551916539905", "1801441962147065857", "1801430840669020162", 0));
        ways.add(new WayDto("1801442786181001218", "1801430840669020162", "1801430457993306114", 0));
        ways.add(new WayDto("1801443038850068482", "1801430840669020162", "1801441962147065857", 0));
        ways.add(new WayDto("1801536302567542785", "1801441962147065857", "1801442421679206402", 0));
        ways.add(new WayDto("1821120279854936066", "1821071654021984257", "1801430161384710145", 0));
        ways.add(new WayDto("1821120298272124929", "1821072243518828546", "1821071654021984257", 0));
        ways.add(new WayDto("1821123244930068482", "1801430161384710145", "1801430457993306114", 0));
        ways.add(new WayDto("1821123368783671298", "1801430457993306114", "1801430161384710145", 0));
        ways.add(new WayDto("1821124563564720129", "1801430161384710145", "1821071654021984257", 0));
        ways.add(new WayDto("1821800679606464513", "1821798184951885826", "1821798392075005954", 0));
        ways.add(new WayDto("1821800749374517250", "1821798392075005954", "1821798184951885826", 0));
        ways.add(new WayDto("1821803231920140290", "1821798392075005954", "1821803090509180929", 0));
        ways.add(new WayDto("1821803271086551041", "1821803090509180929", "1821073230505668610", 0));
        ways.add(new WayDto("1821803298706042882", "1821073230505668610", "1821803090509180929", 0));
        ways.add(new WayDto("1821803320071827457", "1821803090509180929", "1821798392075005954", 0));
        ways.add(new WayDto("1821809371894657025", "1821073230505668610", "1821072243518828546", 0));
        ways.add(new WayDto("1821809460876816385", "1821072243518828546", "1821073230505668610", 0));
        ways.add(new WayDto("1821818011192012801", "1821797679173349377", "1821817904841240577", 1));
        ways.add(new WayDto("1821818093815607297", "1821817904841240577", "1821798184951885826", 0));
        ways.add(new WayDto("1821818112052441089", "1821798184951885826", "1821817904841240577", 0));
        ways.add(new WayDto("1821818211109318657", "1821817904841240577", "1821797679173349377", 0));
        ways.add(new WayDto("1821825411106091010", "1821073230505668610", "1821798392075005954", 0));
        ways.add(new WayDto("1821825528282361857", "1821798392075005954", "1821817904841240577", 0));
        ways.add(new WayDto("1822902942022475777", "1821071654021984257", "1821072243518828546", 0));
        ways.add(new WayDto("1827981667248222209", "1801430161384710145", "1821072243518828546", 0));
        ways.add(new WayDto("1866050557899272193", "1801430161384710145", "1866050332438654978", 0));
        ways.add(new WayDto("1866050613339582465", "1866050332438654978", "1801430161384710145", 1));

        String startId = "1801430457993306114";

        List<String> targetIds = Arrays.asList(
                "1821797679173349377"
        );
        String endId = "1821798184951885826";

        PathPlanner pathPlanner = new PathPlanner(points, ways);
        List<WayDto> result = pathPlanner.planPath(startId, targetIds, endId);
        if (result != null) {
            System.out.print("路径: [");
            for (int i = 0; i < result.size(); i++) {
                System.out.print(result.get(i).toString(pointMap));
                if (i < result.size() - 1) {
                    System.out.print(", ");
                }
            }
            System.out.println("]");
        } else {
            System.out.println("无法到达所有目标点或终点");
        }
    }
}